#include <cmath>
#include "curve.hpp"
#include "message.hpp"

/** @brief Default constructor.

    If this constructor is used, the curve will not be useable until
    another curve is assigned to it.
*/
util::curve::curve() :
  filename(),
  strategy(none),
  samples(0),
  error(0.0)
{}

/** @brief Constructor
    @param filename The file name of the VLAB curve file
    @param strategy The caching strategy to use
    @param samples The number of samples to use in the cache
    @param error The error term for matching x to the curve parameter

    If @a strategy is set to @c none, then the cache is not used, if
    set to @c all, then all vaules for the cache are calculated
    immediately and if set to @c lazy, the cache is filled with a
    value the first time it is used.

    If an unknown strategy is supplied, an error message is issued and
    the curve is not initialised.

    If @a samples is less than two and the caching strategy is not @c
    none, an error message is issued and the curve is not initialised.

    If @a error is not a positive number, an error message is issued
    and the curve is not initialised.
*/
util::curve::curve(std::string filename, util::cache_strategy strategy, unsigned int samples, double error) :
  filename(filename),
  strategy(strategy),
  samples(samples),
  error(error)
{
  switch (strategy) {
  case none:
  case all:
  case lazy:
    reread();
    break;
  default:
    util::err << "util::curve -- An unkown cache strategy was supplied.  The curve was not initialised." << std::endl;
  }

  if (samples < 2 && strategy != none)
    util::err << "util::curve -- There must be at least sample points, but it was " << samples << ".  The curve was not initialised." << std::endl;

  if (error <= 0.0)
    util::err << "util::curve -- The error term must be positive, but it was " << error << ".  The curve was not initialised." << std::endl;
}

/** @brief Copy constructor

    @todo Implement this such that it doesn't use reread, it will be
    more efficient that way.
*/
util::curve::curve(const curve& c) :
  filename(c.filename),
  strategy(c.strategy),
  samples(c.samples),
  error(c.error)  
{
  reread();
}

/** @brief Evaluate the curve a t
    @param t The value to evaluate at

    The parameter @a t is bounded to the range [0,1]
*/
util::point util::curve::operator()(double t) const {
  if (strategy == none)
    return eval(t);

  const double       dt   = t * double(samples);
  const unsigned int low  = (unsigned int)(dt);
  const unsigned int high = low + 1;
  const double       w    = dt - double(low);

  if (strategy == lazy) {
    if (!cache[low].first)  cache[low].second  = eval(t);
    if (!cache[high].first) cache[high].second = eval(t);
  }

  return cache[low].second * (1.0 - w) + cache[high].second * w;
}

/** @brief Find the lenght on the curve between two points in the
           parameter space.
    @param a The first point
    @param b The second point

    @todo Implement the length function
*/
double util::curve::length(double a, double b) const {
  return 0.0;
}

/** @brief Travel along the curve from a point in the parameter space
           by a distance in the curve space.
    @param t Where to start in the parameter space
    @param l The distance to travel by

    @todo Implement the travel function
 */
double util::curve::travel(double t, double l) const {
  return 0.0;
}

/** @brief Find the tangent to the curve
    @param t The place in the curve to find the tangent at

    @todo Implement the tangent function
*/
util::point util::curve::tangent(double t) const {
  return util::point();
}

/** @brief Find the normal to the curve
    @param t The place in the curve to find the normal at

    @todo Implement the normal function
*/
util::point util::curve::normal(double t) const {
  return util::point();
}

/** @brief Reread the contents of the VLAB curve file


    The file reading is designed to be as forgiving as possible, but
    if the file contents are completely non-recognisable, an error is
    issued and the curve will not be initialised.

    @todo Implement the reread function
*/
void util::curve::reread() {
}

/** @brief Reconstruct the cache */
void util::curve::reset_cache() {
  cache.clear();

  switch (strategy) {
  case none: break;
  case lazy:
    cache.assign(samples, std::make_pair(false, util::point()));
    break;
  case all:
    cache.resize(samples);
    for (unsigned int i = 0; i < samples; ++i)
      cache[i] = std::make_pair(true, eval(double(i) / double(samples)));
    break;
  }
}

util::point util::curve::eval(util::d01 t) const {
  const double       k = 1.0 + double(control_points.size() - 3) * t;
  const unsigned int i = int(k) + 2;
  const double       p = k - std::floor(k);

  return
    control_points[i - 3] * basis0(p) +
    control_points[i - 2] * basis1(p) +
    control_points[i - 1] * basis2(p) +
    control_points[i]     * basis3(p);
}

/** @brief A basis function. */
double util::curve::basis0(double t) const {
  return 1.0/6.0 * (-std::pow(t, 3) + 3 * std::pow(t, 2) - 3 * t + 1);
}

/** @brief A basis function. */
double util::curve::basis1(double t) const {
  return 1.0/6.0 * (3 * std::pow(t, 3) - 6 * std::pow(t, 2) + 4);
}

/** @brief A basis function. */
double util::curve::basis2(double t) const {
  return 1.0/6.0 * (-3 * std::pow(t, 3) + 3 * std::pow(t, 2) + 3 * t + 1);
}

/** @brief A basis function. */
double util::curve::basis3(double t) const {
  return 1.0/6.0 * (std::pow(t, 3));
}
